
#include "Algorithms.h"

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <assert.h>

// forward - bacward algorithm
double HMMALG::probOfObservationOnModel(const Observation &obs, unsigned n, unsigned m,
										 const std::vector<double> &pi,
										 const std::vector<std::vector<double> > &a,
										 const std::vector<std::vector<double> > &b) {
	size_t tmax = obs.size();

	std::vector<std::vector<double> > alpha(tmax);
	for (size_t i = 0; i < tmax; i++) {
		alpha[i].resize(n);
	}
	
	for (size_t i = 0; i < n; i++) {
		alpha[0][i] = pi[i] * b[i][obs[0]];
	}

	for (size_t t = 1; t < tmax; t++) {
		for (size_t i = 0; i < n; i++) {
			double sum = 0;
			for (size_t j = 0; j < n; j++) {
				sum += alpha[t - 1][j] * a[j][i];
			}
			alpha[t][i] = sum * b[i][obs[t]];
		}
	}

	double sum = 0;
	for (size_t i = 0; i < n; i++) {
		sum += alpha[tmax - 1][i];
	}

	return sum;
}

double HMMALG::logProbOfObservationOnModel(const Observation &obs, unsigned n, unsigned m,
										 const std::vector<double> &pi,
										 const std::vector<std::vector<double> > &a,
										 const std::vector<std::vector<double> > &b, double eps) {
	size_t tmax = obs.size();
	std::vector<std::vector<double> > alpha(tmax);
	double logP = 0;
	
	for (size_t i = 0; i < tmax; i++) {
		alpha[i].resize(n);
	}
	
	// calculate scaled alpha array
	for (size_t i = 0; i < n; i++) {
		alpha[0][i] = pi[i] * b[i][obs[0]];
	}

	logP -= log(normalize(alpha[0], eps));

	for (size_t t = 1; t < tmax; t++) {
		for (size_t i = 0; i < n; i++) {
			double sum = 0;
			for (size_t j = 0; j < n; j++) {
				sum += alpha[t - 1][j] * a[j][i];
			}
			alpha[t][i] = sum * b[i][obs[t]];
		}
		logP -= log(normalize(alpha[t], eps));
	}

	return logP;
}

double HMMALG::logProb(const std::vector<double> &normfactors) {
	long double logp = 0;
	for (size_t i = 0; i < normfactors.size(); i++) {
		logp -= log(normfactors[i]);
	}
	return logp;
}

void HMMALG::calculateScaledAlpha(const Observation &obs, unsigned n, unsigned m,
		const std::vector<double> &pi,
		const std::vector<std::vector<double> > &a,
		const std::vector<std::vector<double> > &b,
		std::vector<std::vector<double> > &scaled_alpha,
		std::vector<double>& normfactors) {

	size_t tmax = obs.size();
	assert(tmax == scaled_alpha.size());
	assert(tmax == normfactors.size());
	
	// calculate scaled alpha array
	for (size_t i = 0; i < n; i++) {
		scaled_alpha[0][i] = pi[i] * b[i][obs[0]];
	}

	normfactors[0] = normalize(scaled_alpha[0]);

	for (size_t t = 1; t < tmax; t++) {
		assert(scaled_alpha[t].size() == n);
		assert(obs[t] < m);
		for (size_t i = 0; i < n; i++) {
			double sum = 0;
			for (size_t j = 0; j < n; j++) {
				sum += scaled_alpha[t - 1][j] * a[j][i];
			}
			scaled_alpha[t][i] = sum * b[i][obs[t]];
		}
		normfactors[t] = normalize(scaled_alpha[t]);
	}
}

void HMMALG::calculateScaledAlpha(const Observation &obs, unsigned n, unsigned m,
		const std::vector<double> &pi,
		const std::vector<std::vector<double> > &a,
		const std::vector<std::vector<double> > &b,
		std::vector<std::vector<double> > &scaled_alpha) {

	size_t tmax = obs.size();

	// calculate scaled alpha array
	for (size_t i = 0; i < n; i++) {
		scaled_alpha[0][i] = pi[i] * b[i][obs[0]];
	}

	for (size_t t = 1; t < tmax; t++) {
		for (size_t i = 0; i < n; i++) {
			double sum = 0;
			for (size_t j = 0; j < n; j++) {
				sum += scaled_alpha[t - 1][j] * a[j][i];
			}
			scaled_alpha[t][i] = sum * b[i][obs[t]];
		}
	}
}


void HMMALG::calculateScaledBeta(const Observation &obs, unsigned n, unsigned m,
		const std::vector<double> &pi,
		const std::vector<std::vector<double> > &a,
		const std::vector<std::vector<double> > &b,
		std::vector<std::vector<double> > &scaled_beta) {

	size_t tmax = obs.size();

	// calculate scaled beta array
	double scaled_beta_tmax = 1 / double(n);
	for (size_t i = 0; i < n; i++) {
		scaled_beta[tmax - 1][i] = scaled_beta_tmax;
	}
	normalize(scaled_beta[tmax - 1]);

	for (int t = (int)tmax - 2; t >= 0; t--) {
		for (size_t i = 0; i < n; i++) {
			double sum = 0;
			for (size_t j = 0; j < n; j++) {
				sum += scaled_beta[t + 1][j] * a[i][j] * b[j][obs[t + 1]];
			}
			scaled_beta[t][i] = sum;
		}
		normalize(scaled_beta[t]);
	}
}

void HMMALG::calculateGamma(const std::vector<std::vector<double> > &scaled_alpha,
								const std::vector<std::vector<double> > &scaled_beta,
								std::vector<std::vector<double> > &gamma) {
	unsigned tmax = scaled_alpha.size();
	unsigned n = scaled_alpha[0].size();

	for (size_t t = 0; t < tmax; t++) {
		for (size_t i = 0; i < n; i++) {
			gamma[t][i] = scaled_alpha[t][i] * scaled_beta[t][i];
		}
		normalize(gamma[t]);
	}
}

double HMMALG::normalize(std::vector<double> &v) {
	long double sum = 0;
	for (size_t i = 0; i < v.size(); i++) {
		sum += v[i];
	}
	
	long double normfactor = (sum != 0) ? 1 / sum : 0;
	for (size_t i = 0; i < v.size(); i++) {
		v[i] = v[i] * normfactor;
	}
	return normfactor;
}


double HMMALG::normalize(std::vector<double> &v, double eps) {
	long double sum = 0;
	for (size_t i = 0; i < v.size(); i++) {
		if (v[i] < eps) {
			v[i] = eps;
		}
		sum += v[i];
	}
	
	long double normfactor = (sum != 0) ? 1 / sum : 0;
	for (size_t i = 0; i < v.size(); i++) {
		v[i] = v[i] * normfactor;
	}
	return normfactor;
}


void HMMALG::printVector(const std::vector<double> &v) {
	printf("{\n");
	for (size_t i = 0; i < v.size(); i++) {
		printf("%f \n", v[i]);
	}
	printf("} sum = %f \n", sumVector(v));
}

void HMMALG::printIntRow(const std::vector<int> &v) {
	for (size_t i = 0; i < v.size(); i++) {
		printf("%d, ", v[i]);
	}
	printf("\n");
}

void HMMALG::printUnsignedRow(const std::vector<unsigned> &v) {
	for (size_t i = 0; i < v.size(); i++) {
		printf("%d, ", (int) v[i]);
	}
	printf("\n");
}


void HMMALG::printMatrix(const std::vector<std::vector<double> > &v) {
	printf("{\n");
	for (size_t i = 0; i < v.size(); i++) {
		for (size_t j = 0; j < v[i].size(); j++) {
			printf("%f ", v[i][j]);
		}
		printf(" sum = %f \n", sumVector(v[i]));
	}
	printf("}\n");
}

void HMMALG::printMatrix(double **m, unsigned rows, unsigned cols) {
	printf("{\n");
	for (size_t i = 0; i < rows; i++) {
		for (size_t j = 0; j < cols; j++) {
			printf("%f ", m[i][j]);
		}
		printf("\n");
	}
	printf("}\n");
}

double HMMALG::sumArray(double* v, unsigned size) {
	double sum = 0;
	for (size_t i = 0; i < size; i++) {
		sum += v[i];
	}
	return sum;
}

double HMMALG::sumVector(const std::vector<double> &v) {
	double sum = 0;
	for (size_t i = 0; i < v.size(); i++) {
		sum += v[i];
	}
	return sum;
}

int HMMALG::sumVector(const std::vector<int> &v) {
	int sum = 0;
	for (size_t i = 0; i < v.size(); i++) {
		sum += v[i];
	}
	return sum;
}

unsigned HMMALG::sumVector(const std::vector<unsigned> &v) {
	unsigned sum = 0;
	for (size_t i = 0; i < v.size(); i++) {
		sum += v[i];
	}
	return sum;
}

void HMMALG::generate(Observation &obs, unsigned n, unsigned m,
						const std::vector<double> &pi,
						const std::vector<std::vector<double> > &a,
						const std::vector<std::vector<double> > &b) {
	size_t tmax = obs.size();
	size_t state = select(pi, random());
	for (size_t t = 0; t < tmax; t++) {
		obs[t] = select(b[state], random());
		state = (size_t) select(a[state], random());
	}
}

double HMMALG::random() {
	return (double)(rand() % 1000000000) / 1000000000;
}

int HMMALG::select(const std::vector<double> &probs, double treshold) {
	double sum = 0;
	size_t i = 0;
	for (; i < probs.size(); i++) {
		sum += probs[i];
		if (sum > treshold) {
			break;
		}
	}
	return (int) (i % probs.size());
}

double HMMALG::average(const std::vector<int> &v) {
	double sum = sumVector(v);
	return sum / (double)(v.size());
}

double HMMALG::average(const Observation &v) {
	double sum = sumVector(v);
	return sum / (double)(v.size());
}


double HMMALG::dispersion(const std::vector<int> &v) {
	double sum = 0;
	double m = average(v);
	for (size_t i = 0; i < v.size(); i++) {
		double t = v[i] - m;
		sum += t * t;
	}
	return sum / (double)(v.size());
}

double HMMALG::dispersion(const Observation &v) {
	double sum = 0;
	double m = average(v);
	for (size_t i = 0; i < v.size(); i++) {
		double t = v[i] - m;
		sum += t * t;
	}
	return sum / (double)(v.size());
}

void HMMALG::histogram(const std::vector<int> &v, int &min, int &max,
				   std::vector<double> &histogram) {
	size_t n = v.size();
	double c = 1.0 / (double)(n);
	if (n > 0) {
		min = v[0];
		max = v[0];
		for (size_t i = 1; i < n; i ++) {
			if (v[i] < min) {
				min = v[i];
			} else if (v[i] > max) {
				max = v[i];
			}
		}
		
		double d = (double)(max - min + 1) / (double)(histogram.size());
		
		for (size_t i = 0; i < n; i ++) {
			unsigned k = (double)(v[i] - min) / d;
			histogram[k % histogram.size()] += c;
		}
	}
}

void HMMALG::histogram(const Observation &v, unsigned &min, unsigned &max,
				   std::vector<double> &histogram) {
	size_t n = v.size();
	double c = 1.0 / (double)(n);
	if (n > 0) {
		min = v[0];
		max = v[0];
		for (size_t i = 1; i < n; i ++) {
			if (v[i] < min) {
				min = v[i];
			} else if (v[i] > max) {
				max = v[i];
			}
		}
		
		double d = (double)(max - min + 1) / (double)(histogram.size());
		
		for (size_t i = 0; i < n; i ++) {
			unsigned k = (double)(v[i] - min) / d;
			histogram[k % histogram.size()] += c;
		}
	}
}


void HMMALG::findMostProbablePath(const Observation &obs, unsigned n, unsigned m,
										 const std::vector<double> &pi,
										 const std::vector<std::vector<double> > &a,
										 const std::vector<std::vector<double> > &b,
										 std::vector<unsigned> &path) {
	unsigned tmax = obs.size();
	path.resize(tmax);
	
	std::vector<std::vector<double> > delta(tmax);
	// most probable pathes
	std::vector<std::vector<unsigned> > p(tmax);
	for (size_t t = 0; t < tmax; t++) {
		delta[t].resize(n);
		p[t].resize(n);
	}
	
	//init delta
	for (size_t i = 0; i < n; i++) {
		delta[0][i] = pi[i] * b[i][obs[0]];
	}
	
	for (size_t t = 1; t < tmax; t++) {
		for (size_t j = 0; j < n; j++) {
			double max = delta[t - 1][0] * a[0][j];
			unsigned k = 0;
			for (size_t i = 1; i < n; i++) {
				if (delta[t - 1][i] * a[i][j] > max) {
					max = delta[t - 1][i] * a[i][j];
					k = (unsigned) i;
				}
			}
			p[t][j] = k;
			delta[t][j] = max * b[j][obs[t]];
		}
		normalize(delta[t]);
	}
	// restore path frome 'p'
	double max = delta[tmax - 1][0];
	size_t k = 0;
	for (size_t i = 1; i < n; i++) {
		if (delta[tmax - 1][i] > max) {
			max = delta[tmax - 1][i];
			k = i;
		}
	}
	path[tmax - 1] = k;
	for (size_t t = tmax - 1; t > 0; t--) {
		path[t - 1] = p[t][path[t]];
	}
}

void HMMALG::fill(std::vector<double> array, double value) {
	array.assign(array.size(), value);
}

void HMMALG::fill(std::vector<std::vector<double> > array, double value) {
	for (size_t i = 0; i < array.size(); i++) {
		array[i].assign(array.size(), value);
	}
}


